#include <Wire.h>

#define I2C_ADDR                    0x34
#define ADC_BAT_ADDR                0x00
#define MOTOR_TYPE_ADDR             0x14  // 编码电机类型设置
#define MOTOR_ENCODER_POLARITY_ADDR 0x15  // 编码器极性设置
#define MOTOR_FIXED_SPEED_ADDR      0x33  // 固定转速控制（闭环）
#define MOTOR_ENCODER_TOTAL_ADDR    0x3C  // 编码器脉冲数

#define MOTOR_TYPE_JGB37_520_12V_110RPM   3

uint8_t data[20]; // 用于暂存I2C读取数据

// I2C写入数据
bool WireWriteDataArray(uint8_t reg, uint8_t *val, unsigned int len) {
  Wire.beginTransmission(I2C_ADDR);
  Wire.write(reg);
  for (unsigned int i = 0; i < len; i++) {
    Wire.write(val[i]);
  }
  return Wire.endTransmission() == 0;
}

// 读取数据数组
int WireReadDataArray(uint8_t reg, uint8_t *val, unsigned int len) {
  if (!WireWriteDataArray(reg, nullptr, 0)) return -1;
  Wire.requestFrom(I2C_ADDR, len);
  for (unsigned int i = 0; i < len && Wire.available(); i++) {
    val[i] = Wire.read();
  }
  return len;
}

uint8_t MotorType = MOTOR_TYPE_JGB37_520_12V_110RPM;
uint8_t MotorEncoderPolarity = 0;

void setup() {
  Wire.begin(21, 22);
  Serial.begin(9600);
  delay(200);
  
  WireWriteDataArray(MOTOR_TYPE_ADDR, &MotorType, 1);
  delay(5);
  WireWriteDataArray(MOTOR_ENCODER_POLARITY_ADDR, &MotorEncoderPolarity, 1);
}

int32_t EncodeTotal[2];  // **只保留2个电机**

const float wheelDistance = 0.3;
const int8_t V_straight = 50;
const int8_t V_arc = 50;
const float arcRadius = 0.45;

// **调整后，只控制两个电机**
void setMotorSpeeds(int8_t leftSpeed, int8_t rightSpeed) {
  int8_t motorSpeeds[2];  // 只保留两个电机
  motorSpeeds[0] = -leftSpeed;
  motorSpeeds[1] = -rightSpeed;
  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)motorSpeeds, 2);
}

void loop() {
  // 读取电池电压
  uint16_t v;
  WireReadDataArray(ADC_BAT_ADDR, data, 2);
  v = data[0] + (data[1] << 8);
  Serial.print("Battery Voltage = ");
  Serial.print(v);
  Serial.println(" mV");

  // 读取编码器累计脉冲数（仅2个电机）
  WireReadDataArray(MOTOR_ENCODER_TOTAL_ADDR, (uint8_t*)EncodeTotal, 8);
  Serial.printf("Encode1 = %ld  Encode2 = %ld\n", EncodeTotal[0], EncodeTotal[1]);

  // 直线运动
  Serial.println("Straight Line Motion");
  setMotorSpeeds(V_straight, V_straight);
  delay(3000); 

  // 圆弧运动
  Serial.println("Arc Motion (r = 0.45m)");
  float omega = (float)V_arc / arcRadius;
  int8_t v_r = V_arc + (wheelDistance / 2.0) * omega;
  int8_t v_l = V_arc - (wheelDistance / 2.0) * omega;
  Serial.printf("Computed left speed: %d, right speed: %d\n", v_l, v_r);
  setMotorSpeeds(v_l, v_r);
  delay(3000); 

  // 停止
  Serial.println("Stopping");
  int8_t stop[2] = {0, 0};
  WireWriteDataArray(MOTOR_FIXED_SPEED_ADDR, (uint8_t*)stop, 2);
  delay(3000);
}
